/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 *
 * $Id: PositionTypes.hpp,v 1.18 2009-11-02 09:35:21 morsko Exp $
 *
 * \file
 * \brief MaCI - Position Interface Type definition header
 * \author Antti Maula <antti.maula@tkk.fi>
 *
 * \todo When GCC gets the Weffc++ fixed for anonymous unions, 
 *       add 'heading' field to share the space of 'a' field in
 *       the TPose2D structure.
 */
#ifndef _MACI_POSITIONTYPES_HPP_
#define _MACI_POSITIONTYPES_HPP_
#include <inttypes.h>
#include "timestamp.hpp"
#include <math.h>

namespace MaCI {

  /** MaCI - Position interface.
   *
   * This namespace contains all types, methods and helper classes
   * for implementing a Position interface for MaCI.
   */
  namespace Position {

    /** Position element types.
     * 
     * This enumeration defines the available elements for Position interface.
     */
    enum EPositionType { 
      KTypeUnknown                    = -1, ///< Unknown type
      KTypeTimestamp                  = 0,  ///< Contains Common::TTimestamp
      KTypePose2D                     = 1,  ///< Contains TPose2D
      KTypeVariance2D                 = 2,  ///< Contains TVariance2D
      KTypeProbability                = 3,  ///< Contains TProbability
      KTypePoseGeographicDD           = 4,  ///< Contains TPoseGeographicDD
      KTypePoseGeographicDMS          = 5,  ///< Contains TPoseGeographicDMS
      KTypePose2DReference            = 6,  ///< Contains TPose2DReference
      KTypeSpeed2D                    = 7,  ///< Contains TSpeed2D

      KTypePose3D                     = 8,  ///< Contains TPose3D
      KTypeCommand                    = 9,  ///< Contains TCommand
      KTypeSpeed3D                    = 10,  ///< Contains TSpeed3D
      KTypeVariance3D                 = 11,  ///< Contains TVariance3D
      

    };

    /** Position commands.
     *
     * Position commands for the 'TCommand' structure. Defined in global
     * 'Position' namespace because otherwise l^Husers would complain :)
     */
    enum EPositionCommand { 
      KCommandUnknown             = 0, ///< Unkown command
      KCommandInitialize          = 1, ///< Reset
      KCommandSetPose2D           = 2, ///< Set Pose2D on device
      KCommandSetPose3D           = 3  ///< Set Pose3D on device
    };
    
    
    /** 2D Pose.
     *
     * Defines a structure for representing a 2D Pose. 2D-Pose
     * includes the (\p x,\p y) position and \p heading. All units are SI, so
     * coordinates are specified as meters, and heading as
     * radians. Coordinate system is defined as follows:
     * - angles are positive CCW
     * - zero angle is towards the X axis
     * - positive movement moves by the posivite X axis
     * - 'turning left' means turning towards positive Y axis
     *
     * \note The single letter member \p a stands for 'Angle'
     */
    struct TPose2D
    {
      TPose2D(const float aX = 0.0, 
              const float aY = 0.0, 
              const float aHeading = 0.0)
        : x(aX),
          y(aY),
          a(aHeading)       {}

      void NormalizeAngle(){
        if(a > M_PI){
          while(a >  M_PI){
            a-= 2.0 * M_PI;
          }
        }
        else if(a < -M_PI){
          while(a < -M_PI){
            a += 2.0 * M_PI;
          }	
        }
      }


      float x;       ///< X-axis in meters
      float y;       ///< Y-axis in meters
      float a;       ///< Heading in radians (positive CCW, Zero towards X-axis) 
    };


    /** 2D Speed 
     *
     * Defines a structure for representing instantaneous speed and angular
     * speed at a certain position. All units are SI, so speed in m/s and
     * angular speed in rad/s. (positive angular speed turns CCW)
     *
     *
     */
    struct TSpeed2D
    {
      TSpeed2D(const float aSpeed = 0.0,
               const float aAngularSpeed = 0.0)
        : speed(aSpeed),
          angularSpeed(aAngularSpeed){}
      
      float speed;         ///< Speed in m/s
      float angularSpeed;  ///< Angular speed in rad/s
    };


    /** 3D Pose.
     *
     * Defines a structure for representing a 3D Pose. All units are
     * SI, so coordinates are specified as meters, and angle as
     * radians.
     */
    struct TPose3D
    {
      TPose3D(const float aX = 0.0, 
              const float aY = 0.0, 
              const float aZ = 0.0, 
              const float aEul_X = 0.0, 
              const float aEul_Y = 0.0, 
              const float aEul_Z = 0.0)
        : x(aX),
          y(aY),
          z(aZ),
          eul_x(aEul_X),
          eul_y(aEul_Y),
          eul_z(aEul_Z) {}
      float x;       ///< X-axis in meters 
      float y;       ///< Y-axis in meters 
      float z;       ///< Z-axis in meters 
      float eul_x;   ///< Euler angle around X-axis (default 0.0)
      float eul_y;   ///< Euler angle around Y-axis (default 0.0)
      float eul_z;   ///< Euler angle around Z-axis (default 0.0)
    };

    
    /** Pose2D Reference type.
     */
    enum EPose2DReference {
      KReferenceUnknown     = 0, ///< Reference unknown.
      KReferenceOdometry    = 1, ///< Reference is Odometry.
      KReferenceSLAM        = 2, ///< Reference is SLAM.
      KReferenceMap         = 3  ///< Reference is Map.
    };


    /** Pose2D reference container.
     * 
     * This container contains information about the reference of
     * given position element.
     *
     * \todo Jari; is there a more detailed description available?
     *
     */
    struct TPose2DReference
    {
      TPose2DReference(const uint32_t aRef = 0)
        : ref(aRef) {}
      uint32_t ref;
    };


    /** Geographic Coordinates in DMS format (Degrees:Minutes:Seconds)
     *
     *
     * \todo This is not fully defined yet!
     */
    struct TPoseGeographicDMS
    {
      TPoseGeographicDMS(const uint32_t aLa_De,
                         const uint32_t aLa_Mi,
                         const float aLa_Se,
                         const uint32_t aLo_De,
                         const uint32_t aLo_Mi,
                         const float aLo_Se) 
        : latitude_degrees(aLa_De),
          latitude_minutes(aLa_Mi),
          latitude_seconds(aLa_Se),
          longitude_degrees(aLo_De),
          longitude_minutes(aLo_Mi),
          longitude_seconds(aLo_Se) {}
      uint32_t latitude_degrees;
      uint32_t latitude_minutes;
      float latitude_seconds;

      uint32_t longitude_degrees;
      uint32_t longitude_minutes;
      float longitude_seconds;
    };


    /** Geographic Coordinates in DD format (Decimal Degrees).
     * 
     * See:\n
     * http://en.wikipedia.org/wiki/Decimal_degrees\n
     * and\n
     * http://en.wikipedia.org/wiki/Geographic_coordinate_system\n
     * \n
     * For details.\n
     * \n
     * Positive latitudes are north of the equator, negative latitudes
     * are south of the equator. Positive longitudes are east of Prime
     * Meridian, negative longitudes are west of the Prime
     * Meridian. Latitude and longitude are usually expressed in that
     * sequence, latitude before longitude.
     */
    struct TPoseGeographicDD
    {
      TPoseGeographicDD(const float aLat = 0.0, 
                        const float aLong = 0.0)
        : latitude(aLat),
          longitude(aLong) {}
      float latitude;       ///< Latitude coordinate.
      float longitude;      ///< Longtitude coordinate.
    };
    
    
    /** 2D Variance.
     * 
     * Defines the type for representing variance of a 2D Pose.
     * Includes variance for \p x, \p y and \p heading fields.
     * Variance values are expressed in meters and radians.
     */
    typedef TPose2D TVariance2D;

    typedef TPose3D TVariance3D;

    /** Position Probability.
     *
     * Defines the structure for representing a probability value.
     * The probability can be used to indicate the probability that
     * the given positition is what expected.
     *
     * \deprecated This structure is not exactly what is needed.
     * It must be thought over before taking into use. (Jari?)
     */
    struct TProbability
    {
      TProbability(const float aProb = 1.00)
        : probability(aProb) {}
      
      float probability; ///< Probability value ranging 0.00 - 1.00 (means 0% - 100%)
    };
    
    
    /** Position interface command.
     *
     * Describes a Command for the Position interface. 
     * Commands may required additional dataelements, 
     * and are specified in the descriptions of the commands.
     */
    struct TCommand {
      TCommand(EPositionCommand aCmd = KCommandUnknown) 
	: cmd(aCmd) {}
      
      uint32_t cmd; ///< Command value
    };
  }
}

#endif
